//
// Created by vivi on 12/07/2017.
//

#include "HairCore.h"

#define INT32_SIZE sizeof(int32_t)
#define FLOAT_SIZE sizeof(float)

#define READ_INT32(fp_, name_) std::int32_t name_; fp_.read((char*)(&name_), INT32_SIZE);
#define READ_SINGLE(fp_, name_) float name_; fp_.read((char*)(&name_), FLOAT_SIZE);
#define READ_SINGLE_WITHOUT_INIT(fp_, name_) fp_.read((char*)(&name_), FLOAT_SIZE);

namespace {
    inline void trilinear_intp(const float p[3], float weights[8]) {
        float pc[3] = { 1 - p[0], 1 - p[1], 1 - p[2] };
        float w2[4] = { pc[0] * pc[1], p[0] * pc[1], pc[0] * p[1] , p[0] * p[1] };
        weights[0] = w2[0] * pc[2];
        weights[1] = w2[1] * pc[2];
        weights[2] = w2[2] * pc[2];
        weights[3] = w2[3] * pc[2];
        weights[4] = w2[0] * p[2];
        weights[5] = w2[1] * p[2];
        weights[6] = w2[2] * p[2];
        weights[7] = w2[3] * p[2];
    }
}

namespace XRwy {
    using namespace std;
    using namespace Eigen;

    SDFObject::SDFObject(const string file_path, const Affine3f world2local, const float collision_tolerance_, const float impulse_time_) {
        ifstream fin(file_path, ios::in | ios::binary);

        READ_INT32(fin, nx_32);
        this->nx = nx_32;

        READ_INT32(fin, ny_32);
        this->ny = ny_32;

        READ_INT32(fin, nz_32);
        this->nz = nz_32;

        READ_SINGLE_WITHOUT_INIT(fin, origin_x);
        READ_SINGLE_WITHOUT_INIT(fin, origin_y);
        READ_SINGLE_WITHOUT_INIT(fin, origin_z);

        READ_SINGLE_WITHOUT_INIT(fin, dx);

        this->nxy = this->nx * this->ny;

        data = new float[nx * ny * nz];
        fin.read((char *)data, FLOAT_SIZE * nx * ny * nz);

        len_x = (nx - 1) * dx;
        len_y = (ny - 1) * dx;
        len_z = (nz - 1) * dx;
        len_diagonal = sqrt(len_x * len_x + len_y * len_y + len_z * len_z);

        this->current_world2local = world2local;
        this->prev_world2local = world2local;

        this->dist_tol = collision_tolerance_ * len_diagonal;
        this->dist_tol_div_5 = dist_tol / 5.0f;
        this->dist_tol_div_5_1 = 1.0f / dist_tol_div_5;
        this->impulse_time = impulse_time_;
    }

    SDFObject::SDFObject(SDFObject &&rhs) {
        this->current_world2local = rhs.current_world2local;
        this->prev_world2local = rhs.prev_world2local;
        this->dist_tol = rhs.dist_tol;
        this->impulse_time = rhs.impulse_time;
        this->nx = rhs.nx;
        this->ny = rhs.ny;
        this->nz = rhs.nz;
        this->origin_x = rhs.origin_x;
        this->origin_y = rhs.origin_y;
        this->origin_z = rhs.origin_z;
        this->len_diagonal = rhs.len_diagonal;
        this->dx = rhs.dx;
        this->data = rhs.data;
        rhs.data = nullptr;
    }

    float SDFObject::query(const float x, const float y, const float z, Vector3f *grad) const {
        static const float VERY_LARGE = 1e30f;

        float ix = (x - origin_x) / dx;
        float iy = (y - origin_y) / dx;
        float iz = (z - origin_z) / dx;
        int cix = static_cast<int>(ix);
        int ciy = static_cast<int>(iy);
        int ciz = static_cast<int>(iz);

        if (cix <= 0 || ciy <= 0 || ciz <= 0 ||
                cix + 1 >= nx || ciy + 1 >= ny || ciz + 1 >= nz)
            return VERY_LARGE;

        float p[3] = {ix - cix, iy - ciy, iz - ciz};
        float weights[8];
        trilinear_intp(p, weights);

        float dists[] = {
                get_pos(cix, ciy, ciz), get_pos(cix + 1, ciy, ciz), //01
                get_pos(cix, ciy + 1, ciz), get_pos(cix + 1, ciy + 1, ciz), //23
                get_pos(cix, ciy, ciz + 1), get_pos(cix + 1, ciy, ciz + 1), //45
                get_pos(cix, ciy + 1, ciz + 1), get_pos(cix + 1, ciy + 1, ciz + 1) //67
        };

        if (grad) {
            (*grad) << dists[1] - dists[0], dists[2] - dists[0], dists[4] - dists[0];
            grad->normalize();
        }

        float ret = 0.0f;
        for (int i = 0; i < 8; ++i)
            ret += dists[i] * weights[i];

        return ret;
//        if (grad) {
//            (*grad) << x, y, z;
//            grad->normalize();
//        }
//        return sqrt(x * x + y * y + z * z) - 0.5f;
    }

    void SDFObject::fix(Vector3f &pos, Vector3f &vel, const float t_) {
        static const float MIU = 0.5;

        Vector3f model_pos = current_world2local * pos;
        //Vector3f model_vel = current_world2local_mat3x3 * vel;
        Vector3f sdf_vel = pos2model_vel * model_pos;

        //if this point will be out of the model next time, we just ignore the fix
        if (query(model_pos(0), model_pos(1), model_pos(2)) >= dist_tol)
            return;

        Vector3f grad;
	    for (size_t _ = 0; _ < 3; ++_) {
		    float dist = query(model_pos(0), model_pos(1), model_pos(2), &grad);
		    float diff_dist = get_target_dist(dist) - dist;
		    if (diff_dist <= 0)
			    break;

//        Vector3f sdf_vel_normal = sdf_vel.dot(grad) * sdf_vel;
//        Vector3f sdf_vel_tan = sdf_vel - sdf_vel_normal;
//
//        Vector3f model_vel_normal = model_vel.dot(grad) * grad;
//        Vector3f model_vel_tan_rel = (model_vel - model_vel_normal) - sdf_vel_tan;
//
//        Vector3f fixed_model_vel_normal = sdf_vel_normal; //+ (diff_dist / impulse_time) * grad;
//        Vector3f fixed_model_vel_tan =
//                sdf_vel_tan; //+ max(0.0f, 1.0f - MIU * (fixed_model_vel_normal - model_vel_normal).norm() / model_vel_tan_rel.norm()) * model_vel_tan_rel;
//        Vector3f fixed_model_vel = fixed_model_vel_normal + fixed_model_vel_tan;

		    vel = current_local2world_mat3x3 * sdf_vel;
		    pos = current_local2world * (model_pos + diff_dist * grad);
	    }
    }
}